#ifndef CLASS_DG
#define CLASS_DG

#include <stdio.h>
#include <deque>
#include "TreeForceFinder.h"
#include "Vector.h"


#define SQR(x) ((x)*(x))

	
class DG {
public:
	
	typedef long int lint;
	
	REAL r1; int nr;  REAL r2;
	REAL z1; int nz;  REAL z2; 
	
	// derivatives
	INT r1i,r2i,z1i,z2i,dri,dzi, n, _n;

	// Methods
	DG():
	r1(0), nr(100), r2(10),
	z1(0), nz( 50), z2( 5)
	{ update_derivatives(); }
		
	
	void update_derivatives() {
		
		r1i = *(UINT*)&r1;
		r2i = *(UINT*)&r2;
		
		z1i = *(UINT*)&z1;		
		z2i = *(UINT*)&z2;
		
		dri = (r2i-r1i)/nr;
		dzi = (z2i-z1i)/nz;
		
		inv_dr = REAL(nr)/(r2i-r1i); 
		inv_dz = REAL(nz)/(z2i-z1i);
		
		n = nr*nz;
		
		r1jj = _mm_set1_ps(r1);
		r2jj = _mm_set1_ps(r2);
		
		z1jj = _mm_set1_ps(z1);
		z2jj = _mm_set1_ps(z2);
		
		r1ijj = _mm_set1_epi32(r1i);
		r2ijj = _mm_set1_epi32(r2i);
		
		z1ijj = _mm_set1_epi32(z1i);
		z2ijj = _mm_set1_epi32(z2i);
		
		drijj = _mm_set1_epi32(dri);
		dzijj = _mm_set1_epi32(dzi);
		
		inv_drjj = _mm_set1_ps(inv_dr);
		inv_dzjj = _mm_set1_ps(inv_dz);

		nrjj = _mm_set1_epi32(nr);
		nzjj = _mm_set1_epi32(nz);		
		
		
	}
				
	inline void add(TRHO *rho, REAL r, REAL z, REAL value) {
		
		
		REAL rr1(r+r1);
		REAL zz1(z+z1);
		
		int ir = (*(INT*)&rr1-r1i) * inv_dr;
		int iz = (*(INT*)&zz1-z1i) * inv_dz;
		
		if (ir<nr && iz<nz ) { 
			rho[iz*nr+ir]+=value;
		
		
		//printf(" %i %i\n", ir, iz);
		
		}
	}

	//get_acc_ps(__m128 &rjj, __m128 &zjj, __m128 &accrjj, __m128 &acczjj) {
	
	inline void add_ps(TRHO *rho, int *J, __m128 &rjj, __m128 &zjj, REAL *value) {
		
		
		//REAL rr1(r+r1);
		//REAL zz1(z+z1);
		
		//int ir = (*(INT*)&rr1-r1i) * inv_dr;
		//int iz = (*(INT*)&zz1-z1i) * inv_dz;
		
		__m128 rr1jj  = _mm_add_ps(rjj, r1jj); // ok
		__m128 zz1jj  = _mm_add_ps(zjj, z1jj); // ok

		
		__m128i irjj = _mm_cvttps_epi32(_mm_mul_ps(_mm_cvtepi32_ps(_mm_sub_epi32(_mm_castps_si128(rr1jj), r1ijj)), inv_drjj)); // ok
		__m128i izjj = _mm_cvttps_epi32(_mm_mul_ps(_mm_cvtepi32_ps(_mm_sub_epi32(_mm_castps_si128(zz1jj), z1ijj)), inv_dzjj)); // ok

		
		__m128i iznrirjj = _mm_add_epi32(_mm_mullo_epi32(izjj, nrjj), irjj);
		INT iznrir[4];
		_mm_store_si128((__m128i*)iznrir, iznrirjj);
		
		for (int jj=0; jj<4; jj++ )
			rho[ J[jj]*n + iznrir[jj] ]+=value[jj];
		
		//if (ir<nr && iz<nz ) { 
		
		
		
		//printf(" %i %i\n", ir, iz);
		
		//}
	}
	
	

	inline void add_cga(TRHO *rho, REAL r, REAL z, REAL value) {
		
		// I need to shift on half of the current cell size r and z
		
		REAL dr2i;
		REAL dz2i;
		
		//REAL dr3i;
		//REAL dz3i;


		{
			
		REAL rr1(r+r1);
		REAL zz1(z+z1);
		
		REAL ird = (*(INT*)&rr1-r1i) * inv_dr;
		REAL izd = (*(INT*)&zz1-z1i) * inv_dz;

		int ir = ird;
		int iz = izd;
			
		INT ir1i = r1i + ir*dri;
		INT iz1i = z1i + iz*dzi;
 		
		INT ir2i = r1i + (ir+1)*dri;
		INT iz2i = z1i + (iz+1)*dzi;

		//INT ir3i = r1i + (ir+2)*dri;
		//INT iz3i = z1i + (iz+2)*dzi;

		
		REAL r1i = -r1 + *(REAL*)&ir1i;
		REAL z1i = -z1 + *(REAL*)&iz1i;

		REAL r2i = -r1 + *(REAL*)&ir2i;
		REAL z2i = -z1 + *(REAL*)&iz2i;
		
		//REAL r3i = -r1 + *(REAL*)&ir3i;
		//REAL z3i = -z1 + *(REAL*)&iz3i;
		
		
		dr2i = r2i-r1i;
		dz2i = z2i-z1i;
		
		//dr3i = r3i-r2i;
		//dz3i = z3i-z2i;
		
		r-= 0.5*dr2i;
		z-= 0.5*dz2i;
		
		if (r<0) r =0;
		if (z<0) z =0;
		
		}
				
		
		REAL rr1(r+r1);
		REAL zz1(z+z1);
		
		REAL ird = (*(INT*)&rr1-r1i) * inv_dr;
		REAL izd = (*(INT*)&zz1-z1i) * inv_dz;

		int ir = ird;
		int iz = izd;
		
		if (ir<nr-1 && iz<nz-1 ) {
		
			REAL drn = ird-ir;
			REAL dzn = izd-iz;

			int i00 = iz*nr+ir;
			int i10 = iz*nr+(ir+1);
			int i01 = (iz+1)*nr+ir;
			int i11 = (iz+1)*nr+(ir+1);
			
			rho[i00]+= value*(1-drn)*(1-dzn);
			rho[i10]+= value*(drn)*(1-dzn);
			rho[i01]+= value*(1-drn)*(dzn);
			rho[i11]+= value*(drn)*(dzn);

		}
		
	}

	inline int r2ir(REAL r) {
		REAL rr1(r+r1);
		return (*(INT*)&rr1-r1i) * inv_dr;
	}
	/*
	inline int r2ir_ps(__m128 &rjj) {
		
		__m128 rr1jj  = _mm_add_ps(rjj, r1jj); // ok


		
		__m128i irjj = _mm_cvttps_epi32(_mm_mul_ps(_mm_cvtepi32_ps(_mm_sub_epi32(_mm_castps_si128(rr1jj), r1ijj)), inv_drjj)); // ok
		__m128i izjj = _mm_cvttps_epi32(_mm_mul_ps(_mm_cvtepi32_ps(_mm_sub_epi32(_mm_castps_si128(zz1jj), z1ijj)), inv_dzjj)); // ok

		
		
		REAL rr1(r+r1);
		return (*(INT*)&rr1-r1i) * inv_dr;
	}
		*/

	inline int z2iz(REAL z) {
		REAL zz1(z+z1);
		return (*(INT*)&zz1-z1i) * inv_dz;
	}
	
	inline REAL ir2r(REAL ir) {
		INT iri = r1i + ir*dri;
		return -r1 + *(REAL*)&iri;
	}

	inline REAL iz2z(REAL iz) {
		INT izi = z1i + iz*dzi;
		return -z1 + *(REAL*)&izi;
	}
	
	
	inline void add(TRHO *rho, REAL _r, REAL _z, REAL r_, REAL z_, REAL value) {
	
		
		if (r_ < _r) {
			REAL r_tmp(r_);
			REAL z_tmp(z_);
			r_ = _r; z_ = _z;
			_r = r_tmp; _z = z_tmp; 
		}
		
		int _ir(r2ir(_r)), _iz(z2iz(_z));
		int ir_(r2ir(r_)), iz_(z2iz(z_));

		if (_ir==ir_ && _iz==iz_) { 
			if (_ir<nr && _iz<nz ) 
				rho[_iz*nr+_ir]+=value;
			
			return;
		}
		
		
		class Line {
		public:
		   class Node {
			public:
				uint  ir,iz;
				REAL r, z;
				REAL pos;
			};
			
			deque <Node> node;
			REAL r1, r2;
			REAL z1, z2;
			DG &dg;
			
			// derivatives
			REAL dr, dz;
			REAL drZdz, dzZdr;
			REAL l;
			int signr, signz;
			
			Line (REAL _r, REAL _z, REAL r_, REAL z_, DG &_dg):
			r1(_r),
			r2(r_),
			z1(_z),
			z2(z_),
			dg(_dg)
			{
				dr = r2-r1;
				dz = z2-z1;
				drZdz = dr/dz;
				dzZdr = dz/dr;
				l = sqrt(SQR(dr)+SQR(dz));
				signr = dr/fabs(dr);
				signz = dz/fabs(dz);
			}
			
			REAL z(REAL r) {
				return z1 + dzZdr*(r-r1);
			}
			
			REAL r(REAL z) {
				return r1 + drZdz*(z-z1);
			}
			
			
		} line(_r, _z, r_, z_, *this);

		deque < Line::Node > noder;
		deque < Line::Node > nodez;
		//FILE *_fp = fopen("test.tmp.2.txt", "w");

		Line::Node node1, node2;
		
		node1.r = _r; 
		node1.z = _z;
		node1.ir = r2ir(_r);
		node1.iz = z2iz(_z);
		node1.pos = 0;
		
		node2.r = r_; 
		node2.z = z_;
		node2.ir = r2ir(r_);
		node2.iz = z2iz(z_);
		node2.pos = line.l;
		
		
		for (int ir=(_ir<ir_ ? _ir:ir_)+1; ir <= (_ir<ir_ ? ir_:_ir); ir++ ) {
			Line::Node node;
			
			node.r = ir2r(ir);
			node.z = line.z(node.r);
			node.ir = ir;
			node.iz = z2iz(node.z);
			node.pos = sqrt(SQR(node.r-line.r1) + SQR(node.z-line.z1));
			noder.push_back(node);
		}
		
		
		for (int iz=(_iz<iz_ ? _iz:iz_)+1; iz <= (_iz<iz_ ? iz_:_iz); iz++ ) {
			
			Line::Node node;
			
			node.z = iz2z(iz);
			node.r = line.r(node.z);
			node.iz = iz;
			node.ir = r2ir(node.r);
			node.pos = sqrt(SQR(node.r-line.r1) + SQR(node.z-line.z1));
			nodez.push_back(node);

			
		}
	
		
		int signz = (z_-_z)/fabs(z_-_z);
		int signr = (r_-_r)/fabs(r_-_r);
		
		
		
		
		(signz<0) ? nodez.push_front(node2) : nodez.push_back(node2);
		noder.push_back(node2);
	
		int iz = signz<0 ? nodez.size()-1 : 0;
		int ir = signr<0 ? noder.size()-1 : 0;
	
 		
		
		do {
			
			if (noder[ir].pos < nodez[iz].pos) {  
				
				line.node.push_back(noder[ir]);
				
				//printf("noder[%i].pos = %5.1f  %5.1f %i = %i + %i\n", 
					//	 ir, noder[ir].pos, line.l, line.node.size(), noder.size(), nodez.size());

				ir+=signr;
				
			} else {
				
				line.node.push_back(nodez[iz]);
				//printf("nodez[%i].pos = %5.1f  %5.1f %i = %i + %i   %5.1f\n",
					//	 iz, nodez[iz].pos, line.l, line.node.size(), noder.size(), nodez.size(),noder[ir].pos);
				
				iz+=signz;
			}
			
		} while (line.node.size() < noder.size() + nodez.size() -2);

		
		if (node1.r != line.node[0].r || node1.z != line.node[0].z)
			line.node.push_front(node1);
		
		if (node2.r != line.node[line.node.size()-1].r || node2.z!= line.node[line.node.size()-1].z)
			line.node.push_back(node2);

		//exit(0);
		for (int i=0; i<line.node.size()-1; i++) {
			
			REAL l = line.node[i+1].pos - line.node[i].pos;
			rho[line.node[i].iz*nr+line.node[i].ir]+=value*l/line.l;
			
			//printf("%i %g %g\n",i, value*l/line.l, line.node[i].pos);
			
		}

	}
	
	
	void calc(P *p, int n, REAL *rho) {
		
		printf("DG::calc(p, %i, rho)...", n); fflush(stdout);
		/*
		update_derivatives();
		
		memset(rho, 0, nr*nz*sizeof(REAL)); 
			
		for (int i=0; i<n; i++) {
			
			REAL r = sqrt(SQR(p[i].Pos[0]) + SQR(p[i].Pos[1]));
			REAL z = fabs(p[i].Pos[2]);

			int ir = (r-r1)*inv_dr;
			int iz = (z-z1)*inv_dz;
			
			if (ir<nr && iz < nz)
				rho[iz*nr+ir]+= p[i].Mass;
		
		}*/
		printf("done\n");
	}
	
	
	template <typename T>
	void calc(T comp, REAL *rho) {
			
		printf("DG::calc(comp, rho)..."); fflush(stdout);
		
		update_derivatives();
		//print();
		memset(rho, 0, nr*nz*sizeof(*rho));
			
		for (int iz=0; iz<nz; iz++)
		for (int ir=0; ir<nr; ir++) {
			
			INT ir1i = r1i + ir*dri;
			INT iz1i = z1i + iz*dzi;
 		
			INT ir2i = r1i + (ir+1)*dri;
			INT iz2i = z1i + (iz+1)*dzi;

			REAL r1i = -r1 + *(REAL*)&ir1i;
			REAL z1i = -z1 + *(REAL*)&iz1i;

			REAL r2i = -r1 + *(REAL*)&ir2i;
			REAL z2i = -z1 + *(REAL*)&iz2i;

			
			REAL mi = comp.M(r1i, z1i, r2i, z2i, 20, 20);
			
			rho[ iz*nr + ir] = mi;
		}
		
		printf("done\n");
	}

	
	
	void makeAMRGrid(vector< vector<int> > &cell, REAL *m) {
			
			double m_max(0);
			for (int ir=0; ir<nr; ir++)
			for (int iz=0; iz<nz; iz++)
				if (m_max < m[iz*nr+ir]) m_max = m[iz*nr+ir];
				
			printf("m_max = %g\n", m_max);

			calc_cell(cell, m, 0, 0, nr, m_max);
			
			//for (int i=0; i<cell.size(); i++)
				//printf("%i\n", cell[i].size());
			
			_n = cell.size();
	
	}
	
	void mapAMRGrid( REAL *_m, vector< vector<int> > &map, REAL *m) {
		
		memset(_m, 0, _n*sizeof(*_m));
		for (int i=0; i<map.size(); i++)
			for (int j=0; j<map[i].size(); j++)
				_m[i]+=m[map[i][j]];

	}
	

	void calc_cell(vector< vector<int> > &cell, REAL *m, int ir, int iz, int n, REAL m_max){
		
		
		
		double m_cell(0);
		for (int iir=0; iir<n; iir++)
		for (int iiz=0; iiz<n; iiz++)
			m_cell+=m[(iz+iiz)*nr + ir+iir];
		
		//printf("m_cell = %g\n", m_cell);
		
		if (m_max<m_cell && 1<n) {
			
			calc_cell(cell, m, ir,		iz,		n/2, m_max);
			calc_cell(cell, m, ir+n/2,	iz,		n/2, m_max);
			calc_cell(cell, m, ir,		iz+n/2,	n/2, m_max);
			calc_cell(cell, m, ir+n/2,	iz+n/2,	n/2, m_max);
			
		} else {
			
			vector<int> _cell(SQR(n));
			
			for (int iir=0; iir<n; iir++)
			for (int iiz=0; iiz<n; iiz++)
				_cell[iiz*n+iir] = (iz+iiz)*nr + ir+iir;
				
			cell.push_back(_cell);
		}
		
		
	}
	
	
	
	
	void save_txt(const char *filename, REAL *rho){
		
		printf("DG::save_txt('%s')...", filename); fflush(stdout);
		
		update_derivatives();
		
		FILE *fp = fopen(filename, "w");
		
		for (int iz=0; iz<nz; iz++)
		for (int ir=0; ir<nr; ir++) {
							
			INT iri = r1i + (ir+0.5)*dri;
			INT izi = z1i + (iz+0.5)*dzi;

			REAL r = -r1 + *(REAL*)&iri;
			REAL z = -z1 + *(REAL*)&izi;

			fprintf(fp, "%.5e\t%.5e\t%.5e\n", r, z, rho[iz*nr + ir] );
			
		}
		
		fclose(fp);
		
		printf("done\n");
	}

	
	
	
	void print() {
					
		printf("DG::print()\n");
		printf(" ├ r1 = %g\n",  r1);
		printf(" ├ nr = %i\n",  nr);
		printf(" ├ r2 = %g\n",  r2);
		printf(" ├ z1 = %g\n",  z1);
		printf(" ├ nz = %i\n",  nz);
		printf(" └ z2 = %g\n\n",z2);
		
	}

	inline int size() {
		return nr*nz;
	}		

				
private:
		
	REAL inv_dr, inv_dz;
	
	__m128 r1jj, r2jj;
	__m128 z1jj, z2jj;
		
	__m128i r1ijj, r2ijj;
	__m128i z1ijj, z2ijj;
	__m128i drijj, dzijj;
		
	__m128 inv_drjj, inv_dzjj;
	
	__m128i nrjj, nzjj;
};

#endif	
